/**
* detectFace.cpp
*/

#include "LandmarkDetector.h"
#include <iostream>
#include "../util/tinyxml2.h"
#include "cv.h"
#include "highgui.h"
#include "../util/LandmarkType.h"

//#define DISPLAY_DEBUG_WINDOW
using namespace std;
using namespace tinyxml2;

XMLElement* createLandmark(XMLDocument& output, LandmarkType type, Point position);

int main(int argc, char** argv) {
	
	string xmlFilePath;
	if (argc >= 2) {
		xmlFilePath =  argv[1];
	}
	
	// Map photo Id -> path
	map<int, const char*> photos;
	
	// Load photos
	XMLDocument photosInput;
	photosInput.LoadFile((xmlFilePath + "/photos.xml").c_str());

	// Get paths of photoes
	for (XMLElement* photoElement = photosInput.FirstChildElement("photo"); 
		 photoElement != NULL;
	 	 photoElement = photoElement->NextSiblingElement("photo")) {
	
		// Get photo id
		XMLElement* photoIdElement = photoElement->FirstChildElement("id");
	 	XMLText* photoIdText = photoIdElement->FirstChild()->ToText();
	 	int photoId = atoi(photoIdText->Value());

		XMLElement* photoPathElement = photoElement->FirstChildElement("path");
		XMLText* photoPathText = photoPathElement->FirstChild()->ToText();
		const char* photoPath = photoPathText->Value();
	 	
		photos[photoId] = photoPath;
	}

	// Output file
	XMLDocument landmarkOutput;
	XMLDeclaration* declaration = landmarkOutput.NewDeclaration();
	landmarkOutput.InsertEndChild(declaration);

	// Load faces from file
	XMLDocument facesInput;
	facesInput.LoadFile((xmlFilePath + "/faces.xml").c_str());
	
	for (XMLElement* faceElement = facesInput.FirstChildElement("face");
		faceElement != NULL;
		faceElement = faceElement->NextSiblingElement("face")) {
			
		// Get face id
		XMLElement* faceIdElement = faceElement->FirstChildElement("id");
		XMLText* faceIdText = faceIdElement->FirstChild()->ToText();
		int faceId = atoi(faceIdText->Value());
		
		// Get photo id
		XMLElement* photoIdElement = faceElement->FirstChildElement("photo_id");
	 	XMLText* photoIdText = photoIdElement->FirstChild()->ToText();
	 	int photoId = atoi(photoIdText->Value());
		
		// Get top left x coordinate
		XMLElement* element = faceElement->FirstChildElement("top_left_x");
	 	XMLText* elementText = element->FirstChild()->ToText();
	 	int topLeftX = atoi(elementText->Value());
		
		// Get top left y coordinate
		element = faceElement->FirstChildElement("top_left_y");
	 	elementText = element->FirstChild()->ToText();
	 	int topLeftY = atoi(elementText->Value());
		
		// Get bottom right x coordinate
		element = faceElement->FirstChildElement("bottom_right_x");
	 	elementText = element->FirstChild()->ToText();
	 	int bottomRightX = atoi(elementText->Value());
		
		// Get bottom right y coordinate
		element = faceElement->FirstChildElement("bottom_right_y");
	 	elementText = element->FirstChild()->ToText();
	 	int bottomRightY = atoi(elementText->Value());
		cout << "Photo ID: " << photoId << " Face ID: " << faceId << endl;
		cout << "Top Left - X: " << topLeftX << " Y: " << topLeftY << endl;
		cout << "Bottom Right - X: " << bottomRightX << " Y: " << bottomRightY << endl;
		
		// Detect landmarks
		LandmarkDetector detector(photos[photoId], Rect(Point(topLeftX, topLeftY), Point(bottomRightX, bottomRightY)));

		// Create face element for output
		XMLElement* faceLandmarkElement = landmarkOutput.NewElement("face");
		
		// All points
		vector<Point> allPoints;
		
		// Create face id element
		ostringstream stringStream;
		faceIdElement = landmarkOutput.NewElement("face_id");
		stringStream << faceId;
		faceIdText = landmarkOutput.NewText(stringStream.str().c_str());
		faceIdElement->InsertEndChild(faceIdText);
		faceLandmarkElement->InsertEndChild(faceIdElement);
		
		Rect faceRect(Point(topLeftX, topLeftY), Point(bottomRightX, bottomRightY));
		vector<Rect*> mouth = detector.getMouth();
		vector<Rect*> nose = detector.getNose();
		vector<Rect*> eyes = detector.getEyes();
		vector<Rect*> eyePairs = detector.getEyesPair();
		
		// Add forehead
		LandmarkType landmarkType = FOREHEAD;
		int landmarkX = (faceRect.getTopLeft().getX() + faceRect.getBottomRight().getX()) / 2;
		int landmarkY = faceRect.getTopLeft().getY();
		allPoints.push_back(Point(landmarkX, landmarkY));
		
		// Create landmark element
		XMLElement* landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 	faceLandmarkElement->InsertEndChild(landmarkElement);
				
		// Add chin
		landmarkType = CHIN;
		landmarkX = (faceRect.getTopLeft().getX() + faceRect.getBottomRight().getX()) / 2;
		landmarkY = faceRect.getBottomRight().getY();
		allPoints.push_back(Point(landmarkX, landmarkY));
		
		// Create landmark element
		landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
		faceLandmarkElement->InsertEndChild(landmarkElement);
		
		// Add face bottom left
		landmarkType = FACE_BOTTOM_LEFT;
		landmarkX = faceRect.getTopLeft().getX();
		landmarkY = faceRect.getBottomRight().getY();
		allPoints.push_back(Point(landmarkX, landmarkY));
		
		// Create landmark element
		landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 	faceLandmarkElement->InsertEndChild(landmarkElement);
		
		// Add face bottom right
		landmarkType = FACE_BOTTOM_RIGHT;
		landmarkX = faceRect.getBottomRight().getX();
		landmarkY = faceRect.getBottomRight().getY();
		allPoints.push_back(Point(landmarkX, landmarkY));
		
		// Create landmark element
		landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 	faceLandmarkElement->InsertEndChild(landmarkElement);
		
		// Add mouth
		for (vector<Rect*>::const_iterator it = mouth.begin(); it != mouth.end(); it++) {
			landmarkType = MOUTH_LEFT_POINT;
 			
			landmarkX = (*it)->getTopLeft().getX();
			landmarkY = ((*it)->getBottomRight().getY() + (*it)->getTopLeft().getY()) / 2;
			allPoints.push_back(Point(landmarkX, landmarkY));
			
 			// Create landmark element
 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
 			faceLandmarkElement->InsertEndChild(landmarkElement);
			
			landmarkType = MOUTH_RIGHT_POINT;
 			
			landmarkX = (*it)->getBottomRight().getX();
			landmarkY = ((*it)->getBottomRight().getY() + (*it)->getTopLeft().getY()) / 2;
			allPoints.push_back(Point(landmarkX, landmarkY));
			
 			// Create landmark element
 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
 			faceLandmarkElement->InsertEndChild(landmarkElement);
 		}
		
		// Add nose
		for (vector<Rect*>::const_iterator it = nose.begin(); it != nose.end(); it++) {
			landmarkType = NOSE_LEFT_POINT;
 			
			landmarkX = (*it)->getTopLeft().getX();
			landmarkY = ((*it)->getBottomRight().getY() + (*it)->getTopLeft().getY()) / 2;
			allPoints.push_back(Point(landmarkX, landmarkY));
			
 			// Create landmark element
 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
 			faceLandmarkElement->InsertEndChild(landmarkElement);
			
			landmarkType = NOSE_MIDDLE_POINT;
 			
			landmarkX = ((*it)->getTopLeft().getX() + (*it)->getBottomRight().getX()) / 2;
			landmarkY = (*it)->getBottomRight().getY();
			allPoints.push_back(Point(landmarkX, landmarkY));
			
 			// Create landmark element
 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
 			faceLandmarkElement->InsertEndChild(landmarkElement);
			
			landmarkType = NOSE_RIGHT_POINT;
 			
			landmarkX = (*it)->getBottomRight().getX();
			landmarkY = ((*it)->getBottomRight().getY() + (*it)->getTopLeft().getY()) / 2;
			allPoints.push_back(Point(landmarkX, landmarkY));
			
 			// Create landmark element
 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
 			faceLandmarkElement->InsertEndChild(landmarkElement);
 		}
		
		// Add eyes
		Rect* leftEye = NULL;
		Rect* rightEye = NULL;
		for (vector<Rect*>::const_iterator it = eyes.begin(); it != eyes.end(); it++) {
			if (leftEye == NULL) {
				leftEye = (*it);
			}
			else {
				if (leftEye->getTopLeft().getX() < (*it)->getTopLeft().getX()) {
					rightEye = (*it);
				}
				else {
					Rect* tmp = leftEye;
					leftEye = (*it);
					rightEye = tmp;
				}
				
				landmarkType = LEFT_EYE_LEFT_POINT;
				landmarkX = leftEye->getTopLeft().getX();
				landmarkY = (leftEye->getBottomRight().getY() + leftEye->getTopLeft().getY()) / 2;
				allPoints.push_back(Point(landmarkX, landmarkY));
			
	 			// Create landmark element
	 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 			faceLandmarkElement->InsertEndChild(landmarkElement);
				
				landmarkType = LEFT_EYE_RIGHT_POINT;
				landmarkX = leftEye->getBottomRight().getX();
				landmarkY = (leftEye->getBottomRight().getY() + leftEye->getTopLeft().getY()) / 2;
				allPoints.push_back(Point(landmarkX, landmarkY));
			
	 			// Create landmark element
	 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 			faceLandmarkElement->InsertEndChild(landmarkElement);
				
				landmarkType = RIGHT_EYE_LEFT_POINT;
				landmarkX = rightEye->getTopLeft().getX();
				landmarkY = (rightEye->getBottomRight().getY() + rightEye->getTopLeft().getY()) / 2;
				allPoints.push_back(Point(landmarkX, landmarkY));
			
	 			// Create landmark element
	 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 			faceLandmarkElement->InsertEndChild(landmarkElement);
				
				landmarkType = RIGHT_EYE_RIGHT_POINT;
				landmarkX = rightEye->getBottomRight().getX();
				landmarkY = (rightEye->getBottomRight().getY() + rightEye->getTopLeft().getY()) / 2;
				allPoints.push_back(Point(landmarkX, landmarkY));
			
	 			// Create landmark element
	 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
	 			faceLandmarkElement->InsertEndChild(landmarkElement);
			}
 		}
		
		// Add eye pairs
		for (vector<Rect*>::const_iterator it = eyePairs.begin(); it != eyePairs.end(); it++) {
			landmarkType = EYES_MIDDLE;
 			
			landmarkX = ((*it)->getTopLeft().getX() + (*it)->getBottomRight().getX()) / 2;
			landmarkY = ((*it)->getBottomRight().getY() + (*it)->getTopLeft().getY()) / 2;
			allPoints.push_back(Point(landmarkX, landmarkY));
			
 			// Create landmark element
 			landmarkElement = createLandmark(landmarkOutput, landmarkType, Point(landmarkX, landmarkY));
 			faceLandmarkElement->InsertEndChild(landmarkElement);
 		}
 		
 		// Add to file faceLandmarkElement
 		landmarkOutput.InsertEndChild(faceLandmarkElement);
		
#ifdef DISPLAY_DEBUG_WINDOW
		IplImage* image = cvLoadImage(photos[photoId]);
	
		
		for (vector<Point>::const_iterator it = allPoints.begin(); it != allPoints.end(); it++) {
		
			CvPoint topLeft;
			topLeft.x = (*it).getX() - 1;
			topLeft.y = (*it).getY() - 1;
		
			CvPoint bottomRight;
			bottomRight.x = (*it).getX() + 1;
			bottomRight.y = (*it).getY() + 1;
		
			cvRectangle(image, topLeft, bottomRight, CV_RGB(255,0,0), 2, 8, 0);
		}
	
		// Create a new named window with title: result
		cvNamedWindow("result", 1);
	
	    // Show the image in the window named "result"
	    cvShowImage("result", image);
	
		cvWaitKey();
	
		cvDestroyWindow("result");
#endif
	}
	
	// Save output file
	landmarkOutput.SaveFile((xmlFilePath + "/landmarks.xml").c_str());
	
	return 0;
}

XMLElement* createLandmark(XMLDocument& output, LandmarkType type, Point position) {
	// Create landmark element
	XMLElement* landmarkElement = output.NewElement("landmark");
	
	// Create landmark type id element
	ostringstream stringStream;
	XMLElement* landmarkTypeIdElement = output.NewElement("type_id");
	stringStream << type;
	XMLText* landmarkTypeIdText = output.NewText(stringStream.str().c_str());
	landmarkTypeIdElement->InsertEndChild(landmarkTypeIdText);
	landmarkElement->InsertEndChild(landmarkTypeIdElement);
		
	// Create landmark x coordinate
	stringStream.str(""); stringStream.clear();
	XMLElement* landmarkXElement = output.NewElement("x");
	stringStream << position.getX();
	XMLText* landmarkXText = output.NewText(stringStream.str().c_str());
	landmarkXElement->InsertEndChild(landmarkXText);
	landmarkElement->InsertEndChild(landmarkXElement);
	
	// Create landmark y coordinate
	stringStream.str(""); stringStream.clear();
	XMLElement* landmarkYElement = output.NewElement("y");
	stringStream << position.getY();
	XMLText* landmarkYText = output.NewText(stringStream.str().c_str());
	landmarkYElement->InsertEndChild(landmarkYText);
	landmarkElement->InsertEndChild(landmarkYElement);
	
	return landmarkElement;
}
